#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import QuaternionStamped, PointStamped, Vector3Stamped
from tf_transformations import quaternion_from_euler
import math

class DynamicControlPublisher(Node):
    def __init__(self):
        super().__init__('dynamic_control_publisher')
        
        # 创建三个发布器
        self.attitude_pub_ = self.create_publisher(QuaternionStamped, '/bluerov00/attitude_target_desired', 10)
        self.position_pub_ = self.create_publisher(PointStamped, '/bluerov00/position_target', 10)
        self.velocity_pub_ = self.create_publisher(Vector3Stamped, '/bluerov00/velocity_target', 10)
        
        # 声明所有参数
        # 姿态参数
        self.declare_parameter('roll_deg', 0.0)
        self.declare_parameter('pitch_deg', 0.0)
        self.declare_parameter('yaw_deg', 0.0)
        
        # 位置参数
        self.declare_parameter('target_x', 5.0)
        self.declare_parameter('target_y', 5.0)
        self.declare_parameter('target_z', -1.0)
        
        # 速度参数
        self.declare_parameter('target_vx', 0.0)
        self.declare_parameter('target_vy', 0.0)
        self.declare_parameter('target_vz', 0.0)
        
        timer_period = 0.1  # 10Hz
        self.timer = self.create_timer(timer_period, self.publish_all_targets)
        
    def publish_attitude_target(self):
        """发布姿态目标"""
        # 获取当前参数值
        roll_deg = self.get_parameter('roll_deg').value
        pitch_deg = self.get_parameter('pitch_deg').value
        yaw_deg = self.get_parameter('yaw_deg').value
        
        msg = QuaternionStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'map'
        
        # 将角度转换为弧度
        roll_rad = roll_deg * math.pi / 180.0
        pitch_rad = pitch_deg * math.pi / 180.0
        yaw_rad = yaw_deg * math.pi / 180.0
        
        # 使用 tf_transformations 库将欧拉角转换为四元数
        quaternion = quaternion_from_euler(roll_rad, pitch_rad, yaw_rad)
        
        msg.quaternion.x = quaternion[0]
        msg.quaternion.y = quaternion[1]
        msg.quaternion.z = quaternion[2]
        msg.quaternion.w = quaternion[3]
        
        self.attitude_pub_.publish(msg)
        self.get_logger().info(f'Attitude - Roll: {roll_deg}°, Pitch: {pitch_deg}°, Yaw: {yaw_deg}°')
    
    def publish_position_target(self):
        """发布位置目标"""
        # 获取当前参数值
        target_x = self.get_parameter('target_x').value
        target_y = self.get_parameter('target_y').value
        target_z = self.get_parameter('target_z').value
        
        msg = PointStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'world'
        
        msg.point.x = target_x
        msg.point.y = target_y
        msg.point.z = target_z
        
        self.position_pub_.publish(msg)
        self.get_logger().info(f'Position - X: {target_x:.2f}m, Y: {target_y:.2f}m, Z: {target_z:.2f}m')
    
    def publish_velocity_target(self):
        """发布速度目标"""
        # 获取当前参数值
        target_vx = self.get_parameter('target_vx').value
        target_vy = self.get_parameter('target_vy').value
        target_vz = self.get_parameter('target_vz').value
        
        msg = Vector3Stamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'world'
        
        msg.vector.x = target_vx
        msg.vector.y = target_vy
        msg.vector.z = target_vz
        
        self.velocity_pub_.publish(msg)
        self.get_logger().info(f'Velocity - Vx: {target_vx:.2f}m/s, Vy: {target_vy:.2f}m/s, Vz: {target_vz:.2f}m/s')
    
    def publish_all_targets(self):
        """发布所有控制目标"""
        self.publish_attitude_target()
        self.publish_position_target()
        self.publish_velocity_target()
        self.get_logger().info('---')  # 分隔线

def main():
    rclpy.init()
    node = DynamicControlPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()